/**
 * @file        motor_ctrl.c
 * @brief       电机控制的核心文件。状态机与错误处理、设定目标值
 * @version     V1.2
 * @author      Jsir2022 (jsir2022@outlook.com)
 * @date        2024-02-28 18:14:42
 * LastEditors  Jsir2022
 * LastEditTime 2024-03-03 22:56:30
 * history		版本	日期		作者		说明
 * 				V1.0	2024-2-28	Jsir2022	正式发布
 *              V1.1	2024-2-29	Jsir2022	添加了错误处理
 *              V1.2	2024-3-2	Jsir2022	FOC算法验证成功，强拖可以
 * Copyright (c) 2024 by Jsir2022, All Rights Reserved.
 * @verbatim
 * ==============================================================================
 *
 * ==============================================================================
 * @endverbatim
 */
#include "motor_ctrl.h"
#include "encoder.h"

#include "bsp_uart.h"

#include "tim.h"

stMotorCtrl_type MC;

void Motor_Ctrl_initSystem(void)
{
    MC.mtPara.Rs = 0.037745f;      // 37.745mR
    MC.mtPara.Ls = 0.00000898277f; // 8.982766666uH
    MC.mtPara.pairs = 11;

    MC.mePara.dir = +1;
    MC.focPara.v_d = 0.0f;
    MC.focPara.v_q = 0.3f;

    Motor_Drive_initHardware();
    ENCODER_init();

}
void Motor_Ctrl_runState(void)
{
    // Motor_Enco_readTheta(&MC.mePara, MC.mtPara.pairs);
    ENCODER_loop();
    // MC.focPara.theta = Encoder.phase;
    // MC.focPara.theta = Motor_Enco_readTheta(&MC.mePara, MC.mtPara.pairs);
    // MC.focPara.v_q = -MC.mePara.speed * 100;//阻尼模式，反向速度乘以阻尼系数使得Vq
    // MC.focPara.v_q = MC.mePara.speed * 30; // 动能回收？
    // Vq大于0且小于反电动势（正向旋转，反电动势 正值），能保证产生负的iq，这就保证了功率是负的，
    // Vq小于0且大于反电动势（反向旋转，反电动势 负值），能保证产生正的iq，这就也保证了功率是负的，
    // Motor_Enco_caliSpeed(&MC.mePara);
    Motor_Ctrl_runMode();
    Motor_FOC_calcAll(&MC.focPara);
    Motor_Drive_setPWM(MC.focPara.T_deltaA, MC.focPara.T_deltaB, MC.focPara.T_deltaC);

    switch ((uint8_t)MC.mtState)
    {
    case __Motor_Stop:

        break;
    }
}

void Motor_Ctrl_runMode(void)
{
    // MC.focPara.theta += 0.01f;
    // if (MC.focPara.theta > M_2PI)
    //     MC.focPara.theta = 0.0f;
    // else if (MC.focPara.theta < 0)
    //     MC.focPara.theta = M_2PI;

    switch ((uint8_t)MC.Mode)
    {
    case dragvf_mode:

        break;
    }
}

void Motor_Ctrl_process(void)
{
    // DMA_Printf("%f\n", MC.mePara.mec_angle);
    // DMA_Printf("%f,%f\n", MC.mePara.speed, MC.focPara.theta);
    // DMA_Printf("%f,%f,%f\n", MC.focPara.T_deltaA, MC.focPara.T_deltaB, MC.focPara.T_deltaC);
    // DMA_Printf("%f,%f,%f\n", MC.focPara.i_a, MC.focPara.i_b, MC.focPara.i_c);
    // DMA_Printf("%f,%f\n", MC.focPara.v_q, MC.focPara.i_q);
    DMA_Printf("%d,%f,%f\n", MT6825.angle,Encoder.phase, Encoder.phase_vel);
    
}

void Motor_Ctrl_errorHandler(emMotorCtrl_Error_type _Error)
{
    switch ((uint8_t)_Error)
    {
    case __Enco_No_Mag:
        break;
    }
}